﻿module Physics.Chapter5.IntervalSweep

open Casanova
open Casanova.Input
open Casanova.Core
open Casanova.Coroutines
open Casanova.Utilities
open Casanova.Math
open Casanova.Game
open Casanova.Drawing
open Casanova.StandardLibrary
open Casanova.StandardLibrary.Core
open Physics.Chapter5.RigidBody
open Physics.Chapter5.CollisionResponse
open Physics.Chapter5.Geometry

open Microsoft.Xna.Framework

type ExtremeType = Begin = 0 | End = 1

let find_intersections (boxes:BoundingBox[]) =
  let sorted get_min get_max = 
    [| 
      for i = 0 to boxes.Length - 1 do
        let b = boxes.[i]
        yield get_min b, ExtremeType.Begin, i
        yield get_max b, ExtremeType.End, i
    |]
  let sorted_x = sorted (fun b -> b.Min.X) (fun b -> b.Max.X) |> Array.sortBy (fun (x,_,_) -> x)
  let sorted_y = sorted (fun b -> b.Min.Y) (fun b -> b.Max.Y) |> Array.sortBy (fun (x,_,_) -> x)
  let sorted_z = sorted (fun b -> b.Min.Z) (fun b -> b.Max.Z) |> Array.sortBy (fun (x,_,_) -> x)

  let links =
    [|
      for b in boxes do
        yield 
          [|
            for b in boxes do
              yield 0
          |]
    |]
  let result = ResizeArray()
  let add_link i j =
    let i,j = min i j, max i j
    links.[i].[j] <- links.[i].[j] + 1
    if links.[i].[j] = 3 then
      result.Add(i,j)

  let find_collisions(sorted:_[]) =
    let fst (a,b,c) = a
    let snd (a,b,c) = b
    let trd (a,b,c) = c
    let current_group = ResizeArray()
    let mutable current_box = sorted.[0]
    current_group.Add(current_box |> trd)
    for i = 1 to sorted.Length - 1 do
      let b_i = sorted.[i]
      if b_i |> snd = ExtremeType.Begin then
        current_group.Add(b_i |> trd)
      else
        let j = current_group |> Seq.findIndex ((=) (b_i |> trd))
        do current_group.RemoveAt j
        for b_j in current_group do
          do add_link(b_i |> trd) b_j
  
  do find_collisions sorted_x
  do find_collisions sorted_y
  do find_collisions sorted_z

  result
